#include "ros/ros.h"
#include "cyberarm_controller/me_msg.h"


#define MOTOR_NUM 4

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ROS_INFO("motor_state_node");
    ros::init(argc, argv, "motor_state_node");
    ros::NodeHandle nh;
    ros::Publisher motor_state_pub = nh.advertise<cyberarm_controller::me_msg>("motor_enable", 10); 
    bool flag = std::stoi(argv[1]);
    
    ros::Rate rate_delay(1); // 延时1s等待CAN接口初始化之后再进行reply_state配置，否则可能会出现配置不成功的情况
    rate_delay.sleep();
    ros::Rate step(20);
    int times = 0;
    while(ros::ok() && times < 10)
    {
        //! todo: 循环十次设置state
        times++;
        for(unsigned int i = 0; i < MOTOR_NUM; i++)
        {
            cyberarm_controller::me_msg i_message;
            i_message.id = i + 1;
            if(flag == true)
            {
                //！ enable
                i_message.cmd_mode = 3;  // 电机enable
            }
            else
            {
                //! disable
                i_message.cmd_mode = 4;  // 电机enable
            }
            motor_state_pub.publish(i_message);
            step.sleep();
        }
        ros::spinOnce();
    }

    if(flag == true)
    {
        ROS_INFO("motor enable!");
    }
    else
    {
        ROS_INFO("motor disable!");
    }

    return 0;
}
